#include "huang_onepass.h"
#include "modbus_tcp_user.h"
#include "axis.h"
#include "bsp.h"
#include <string.h>
#include "control.h"
#include "sys.h"

#define Z_AXIS AXIS1
#define X_AXIS AXIS3
#define Y_AXIS AXIS2



// static USHORT   *usRegInputBuf;
// static USHORT   *usRegHoldingBuf;
// static BOOL     *usRegCoilsBuf;
// static BOOL     *usRegDiscreteBuf;
static float    *x_press_ink_pos;
static float    *x_clean_start_pos;
static float    *x_clean_end_pos;
static float    *cap_press_ink_pos;
static float    *cap_clean_pos;
static float    *coor_x;
static float    *coor_ink;
static float    *x_clean_speed;
static float    *ink_clean_speed;
static float    *print_x_coor;
static float    *print_x_speed;
static float    *print_y_coor;
static float    *print_y_speed;
static float    *print_z_pos;
static uint8_t WaitActBefMove(uint8_t axis) 
{ 
    if(axis == X_AXIS)
    {
        mt_move_home(Z_AXIS,axis_info[Z_AXIS].axis_param.homeSpeed);
        if(axis_info[Z_AXIS].move_info->move_rsp == MOVE_STOP)
        {
            return 1;
        }
    }
    return 0; 
}

static uint8_t VendorInit(void) 
{ 
    // usRegCoilsBuf = modbus_get_coils_buf();
    // usRegDiscreteBuf = modbus_get_discrete_buf();
    // usRegHoldingBuf = modbus_get_holding_buf();
    // usRegInputBuf = modbus_get_input_buf();

    x_press_ink_pos = (float*)&usRegHoldingBuf[31];
    x_clean_start_pos = (float*)&usRegHoldingBuf[33];
    x_clean_end_pos = (float*)&usRegHoldingBuf[35];
    cap_press_ink_pos = (float*)&usRegHoldingBuf[39];
    cap_clean_pos = (float*)&usRegHoldingBuf[41];
    coor_x = (float*)&usRegInputBuf[0];
    coor_ink = (float*)&usRegInputBuf[2];
    x_clean_speed = (float*)&usRegHoldingBuf[37];
    ink_clean_speed = (float*)&usRegHoldingBuf[43];
    print_x_coor = (float*)&usRegHoldingBuf[47];
    print_x_speed = (float*)&usRegHoldingBuf[49];
    print_y_coor = (float*)&usRegHoldingBuf[51];
    print_y_speed = (float*)&usRegHoldingBuf[53];
    print_z_pos = (float*)&usRegHoldingBuf[55];

    g_pump_info[0].pump_input = IN13;
    g_pump_info[0].pump_out = OUT1;
    g_pump_info[0].pump_status = 0;
    g_pump_info[1].pump_input = IN14;
    g_pump_info[1].pump_out = OUT2;
    g_pump_info[1].pump_status = 0;
    g_pump_info[2].pump_input = IN15;
    g_pump_info[2].pump_out = OUT3;
    g_pump_info[2].pump_status = 0;
    g_pump_info[3].pump_input = IN16;
    g_pump_info[3].pump_out = OUT4;
    g_pump_info[3].pump_status = 0;

    g_pump_spill = IN6;
    return 0; 
}

static uint8_t OnMainLoop(void) 
{ 
    float pos_acc;
    float speed_acc;
    g_pump_info[0].pump_timeout = usRegHoldingBuf[46];
    g_pump_info[1].pump_timeout = usRegHoldingBuf[46];
    g_pump_info[2].pump_timeout = usRegHoldingBuf[46];
    g_pump_info[3].pump_timeout = usRegHoldingBuf[46];

    usRegInputBuf[16] = g_pump_info[0].pump_status;
    usRegInputBuf[17] = g_pump_info[1].pump_status;
    usRegInputBuf[18] = g_pump_info[2].pump_status;
    usRegInputBuf[19] = g_pump_info[3].pump_status;
    
    static uint8_t press_ink = 0;
    if(press_ink != usRegCoilsBuf[37])//ѹī
    {
        press_ink = usRegCoilsBuf[37];
        if(press_ink)
        {
            bsp_set_out(OUT16,GPIO_PIN_SET);
            bsp_set_out(OUT15,GPIO_PIN_SET);
        }
        else
        {
            bsp_set_out(OUT16,GPIO_PIN_RESET);
            bsp_set_out(OUT15,GPIO_PIN_RESET);
        }
    }

    static uint8_t set_x_press_ink_pos = 0;
    if(set_x_press_ink_pos != usRegCoilsBuf[32])
    {
        set_x_press_ink_pos = usRegCoilsBuf[32];
        if(set_x_press_ink_pos)
        {
            memcpy(x_press_ink_pos,coor_x,sizeof(float));
        }
    }

    static uint8_t set_x_clean_start_pos = 0;
    if(set_x_clean_start_pos != usRegCoilsBuf[33])
    {
        set_x_clean_start_pos = usRegCoilsBuf[33];
        if(set_x_clean_start_pos)
        {
            memcpy(x_clean_start_pos,coor_x,sizeof(float));
        }
    }

    static uint8_t set_x_clean_end_pos = 0;
    if(set_x_clean_end_pos != usRegCoilsBuf[34])
    {
        set_x_clean_end_pos = usRegCoilsBuf[34];
        if(set_x_clean_end_pos)
        {
            memcpy(x_clean_end_pos,coor_x,sizeof(float));
        }
    }

    static uint8_t set_cap_press_ink_pos = 0;
    if(set_cap_press_ink_pos != usRegCoilsBuf[35])
    {
        set_cap_press_ink_pos = usRegCoilsBuf[35];
        if(set_cap_press_ink_pos)
        {
            memcpy(cap_press_ink_pos,coor_ink,sizeof(float));
        }
    }

    static uint8_t set_cap_clean_pos = 0;
    if(set_cap_clean_pos != usRegCoilsBuf[36])
    {
        set_cap_clean_pos = usRegCoilsBuf[36];
        if(set_cap_clean_pos)
        {
            memcpy(cap_clean_pos,coor_ink,sizeof(float));
        }
    }

    static uint8_t y_start_bt = 0;
    if(y_start_bt != usRegCoilsBuf[39])
    {
        y_start_bt = usRegCoilsBuf[39];
        if(y_start_bt)
        {
            memcpy(&pos_acc,print_y_coor,sizeof(float));
            int64_t print_y_pos =  (int64_t)(pos_acc / 25.4f * (float)axis_info[Y_AXIS].axis_param.resolution);
            memcpy(&speed_acc,print_y_speed,sizeof(float));
            uint32_t speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[Y_AXIS].axis_param.resolution);
            mt_movelen_r(Y_AXIS,print_y_pos,speed);
        }
    }
    static uint8_t y_stop_bt = 0;
    if(y_stop_bt != usRegCoilsBuf[40])
    {
        y_stop_bt = usRegCoilsBuf[40];
        if(y_stop_bt)
        {
            mt_stop(Y_AXIS,0);
        }
    }

    memcpy(&pos_acc,cap_press_ink_pos,sizeof(float));
    axis_info[Z_AXIS].axis_param.maxLen = (uint64_t)(pos_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    return 0; 
}


static uint8_t VendorAutoClean(void) 
{ 
    printf("VendorAutoClean in\n");
    int64_t dest;
    uint32_t speed;
    float pos_acc;
    float speed_acc;
    memcpy(&pos_acc,x_press_ink_pos,sizeof(float));
    memcpy(&speed_acc,x_clean_speed,sizeof(float));
    printf("x_press_ink_pos %f x_clean_speed %f\n",pos_acc,speed_acc);
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    mt_moveto(X_AXIS,dest,speed);
    memcpy(&pos_acc,cap_press_ink_pos,sizeof(float));
    memcpy(&speed_acc,ink_clean_speed,sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    mt_moveto(Z_AXIS,dest,speed);
    printf("VendorAutoClean press ink\n");
    osDelay(1000);

    bsp_set_out(OUT16,GPIO_PIN_SET);
    bsp_set_out(OUT15,GPIO_PIN_SET);
    osDelay(usRegHoldingBuf[45]);
    bsp_set_out(OUT16,GPIO_PIN_RESET);
    osDelay(2000);
    bsp_set_out(OUT15,GPIO_PIN_RESET);

    osDelay(1000);

    mt_moveto(Z_AXIS,0,speed);
    memcpy(&pos_acc,x_clean_start_pos,sizeof(float));
    memcpy(&speed_acc,x_clean_speed,sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    mt_moveto(X_AXIS,dest,speed);
    printf("VendorAutoClean cleaning\n");

    memcpy(&speed_acc,ink_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    memcpy(&pos_acc,cap_clean_pos,sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    mt_moveto(Z_AXIS,dest,speed);

    memcpy(&pos_acc,x_clean_end_pos,sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    memcpy(&speed_acc,x_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    mt_moveto(X_AXIS,dest,speed);

    memcpy(&speed_acc,ink_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    mt_moveto(Z_AXIS,0,speed);

    memcpy(&speed_acc,x_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    mt_moveto(X_AXIS,0,speed);
    
    memcpy(&pos_acc,cap_press_ink_pos,sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    memcpy(&speed_acc,ink_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    mt_moveto(Z_AXIS,dest,speed);

    printf("VendorAutoClean out\n");
    return 1; 
}
static void vender_thread(void *arg)
{    
    float pos_acc;
    float speed_acc;
    while(1)
    {
        static uint8_t printf_x_bt = 0;
        if(printf_x_bt != usRegCoilsBuf[38])//ѹī
        {
            printf_x_bt = usRegCoilsBuf[38];
            if(printf_x_bt)
            {
                memcpy(&pos_acc,print_x_coor,sizeof(float));
                int64_t print_x_pos =  (int64_t)(pos_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
                memcpy(&speed_acc,print_x_speed,sizeof(float));
                uint32_t speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
                mt_move_home(Z_AXIS,axis_info[Z_AXIS].axis_param.homeSpeed);
                mt_moveto(X_AXIS,print_x_pos,speed);
                memcpy(&pos_acc,print_z_pos,sizeof(float));
                int64_t print_z_pos =  (int64_t)(pos_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
                mt_moveto(Z_AXIS,print_z_pos,axis_info[Z_AXIS].axis_param.homeSpeed);
                
            }
        }
        osDelay(100);
    }
}

static uint8_t DoWetAction(void) 
{     
    printf("DoWetAction in\n");
    int64_t dest;
    uint32_t speed;
    float pos_acc;
    float speed_acc;
    memcpy(&speed_acc,x_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[X_AXIS].axis_param.resolution);
    mt_moveto(X_AXIS,0,speed);

    memcpy(&pos_acc,cap_press_ink_pos,sizeof(float));
    dest = (int64_t)(pos_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    memcpy(&speed_acc,ink_clean_speed,sizeof(float));
    speed = (uint32_t)(speed_acc / 25.4f * (float)axis_info[Z_AXIS].axis_param.resolution);
    mt_moveto(Z_AXIS,dest,speed);

    sys_thread_new("vender_thread", vender_thread, NULL, 512, 0);
    return 1; 
}

static uint8_t MotionReady(void) 
{
    DoWetAction();
		return 0;
}

void huang_onepass2_init(void)
{
    VendorSpec.VendorInit = VendorInit;
    VendorSpec.WaitActBefMove = WaitActBefMove;
    VendorSpec.OnMainLoop = OnMainLoop;
    VendorSpec.VendorAutoClean = VendorAutoClean;
    VendorSpec.DoWetAction = DoWetAction;
    VendorSpec.MotionReady = MotionReady;
    // VendorSpec.DoOutWetAction = DoWetOutActionDefault;
}

